"""
方案一：1.如果激光离所有的雷达点位置都大于某一个阈值，那么激光这个点就是一个独立的点
       2.如果激光离部分或者全部点小于这个阈值，那么取最小值的那个雷达点认为和激光是同一个点
"""

from numpy import linalg as la
import numpy as np


def computeDis(laser, radar):
    return la.norm(laser - radar)


def judgeBySimple(laser, radar):
    if len(laser) == 0 and len(radar) == 0:
        return np.array([[0.0, 0.0]])
    elif len(laser) == 0:
        return radar
    elif len(radar) == 0:
        return laser
    else:
        all_dis = []
        count = []
        for i in range(radar.shape[0]):
            dis = computeDis(laser, radar[i])
            count.append(dis)
            if dis < 4:
                all_dis.append(dis)
        if not all_dis:
            return np.row_stack((radar, laser))
        else:
            target = count.index(min(all_dis)) + 1
            radar[target - 1] = (radar[target - 1] + laser) / 2
            return radar
